#ifndef __VECTOR3_H
#define __VECTOR3_H

#include <cmath>
#include <iostream>
#include <cassert>
#include <string>

class Vector3 {
 public:
	double v[3];

	Vector3(double onevalue = 0) {
		for (int i=0; i<2; i++) {
			v[i] = onevalue;
		}		
	}

	Vector3(double x, double y, double z) {
		v[0] = x;
		v[1] = y;
		v[2] = z;
	}

	void set(double x, double y, double z) {
		v[0] = x;
		v[1] = y;
		v[2] = z;
	}
	
	double get(int i) const {
		assert(i>=0 && i<=2);
		return v[i];
	}

	const double& operator[] (int i) const
	{
		return v[i];
	}
 
	void add(Vector3 &a) {
		for (int i=0; i<3; i++) {
			v[i] += a.v[i];
		}
	}

	static Vector3 add(Vector3 &a, Vector3 &b) {
		Vector3 result(0,0,0);
		result.add(a);
		result.add(b);
		return result;
	}

	Vector3 operator+(const Vector3 &rhs) const
	{
		return Vector3(v[0] + rhs.v[0], v[1] + rhs.v[1], v[2] + rhs.v[2]);
	}

	void subtractInPlace(Vector3 &a) {
		for (int i=0; i<3; i++) {
			v[i] -= a.v[i];
		}
	}

	static Vector3 subtract(Vector3 &a, Vector3 &b) {
		Vector3 result(a);
		result.subtractInPlace(b);
		return result;
	}

	Vector3 operator-(Vector3 rhs) const
	{
		return Vector3(v[0] - rhs.v[0], v[1] - rhs.v[1], v[2] - rhs.v[2]);
	}

	void scale(int factor) {
		for (int i=0; i<3; i++) {
			v[i] *= factor;
		}
	}

	Vector3 operator*(const double &rhs) const
	{
		return Vector3(rhs*v[0], rhs*v[1], rhs*v[2]);
	}

	Vector3 operator*(const Vector3 &rhs) const
	{
		return Vector3::cross(*this, rhs);
	}

	Vector3 operator/(const double &rhs) const
	{
		return Vector3(v[0]/rhs, v[1]/rhs, v[2]/rhs);
	}

	double innerProduct(const Vector3& rhs) const
	{
		return    v[0] * rhs.v[0] 
	        	+ v[1] * rhs.v[1] 
			+ v[2] * rhs.v[2];
	}

	double dot(const Vector3& rhs)
	{
		return    v[0] * rhs.v[0] 
	        	+ v[1] * rhs.v[1] 
			+ v[2] * rhs.v[2];
	}	

	static double dot(const Vector3& lhs, const Vector3& rhs)
	{
		return    lhs.v[0] * rhs.v[0] 
	        	+ lhs.v[1] * rhs.v[1] 
			+ lhs.v[2] * rhs.v[2];
	}

	static Vector3 cross(const Vector3& a, const Vector3& b) 
	{
		double x = a.v[1]*b.v[2] - a.v[2]*b.v[1];
		double y = a.v[2]*b.v[0] - a.v[0]*b.v[2];
		double z = a.v[0]*b.v[1] - a.v[1]*b.v[0];
		
		return Vector3(x,y,z);
	}

	double length() const 
	{
		return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
	}

	void normalize() {
		double len = length();
		if (len==0) return; // we don't like diving by zero around here.

		for (int i=0; i<3; i++) {
			v[i] = v[i]/len;
		}
	}

	static Vector3 normalize(const Vector3 &vec) {
		return vec * (1.0f/vec.length());
	}

	void print(std::string title="vector") {
		std::cout << title << " = [ ";
		for (int i=0; i<3; i++) {
			std::cout << v[i] << " ";
		}
		std::cout << "]" << std::endl;
	}

	void copy(const Vector3 &vec) {
		v[0] = vec[0];
		v[1] = vec[1];
		v[2] = vec[2];
	}
};

bool operator==(const Vector3 &vec, const Vector3 &othervec) {
	return vec[0] == othervec[0] && vec[1] == othervec[1] && vec[2] == othervec[2];
}

Vector3 operator*(double constant, const Vector3 &vector) {
	return vector*constant;
}

#endif
